<h2>Physics Tutorial 3: Robots</h2>
<p>
	In this tutorial, we set up a robots simulation in which two circular gray robots
	attempt to collect several blue cans and move them to the top of the screen. The
	robots simulation uses all the main features of the system - there are friction forces, constraints,
	and collisions.
</p>
<p>
	The robots are simulated with a circular body and an effector. The effector is two
	small circles that are constrained to the body using pin joints. In order to pick up
	a can, the robot needs to approach the can so that the can contacts the robot's body
	between the effectors. The <b>Effector</b> class is already defined 
	for you because it doesn't do anything interesting on its own.
</p>
<p>
	This tutorial teaches:
	<ul>
		<li>How to create complex shapes by constraining simple shapes together</li>
		<li>How to use the <b>sim.robot2D.Robot</b> library to perform coordinate transformations
			and robotic control</li>
		<li>How to turn off collisions between a pair of objects</li>
		<li>How to perform actions in response to a collision</li>
		<li>How to dynamically determine
			the type collision response at the time of a collision</li>
		<li>How to dynamically register and un-register constraints</li>
	</ul>
</p>

<h2>Create the <b>Can</b> class</h2>
<p>
	In the <b>sim/app/physicsTutorial3</b> directory, create a file called <b>Can.java</b>.
	In this file add:
</p>
<pre>
package sim.app.physicsTutorial3;

import java.awt.*;
import sim.engine.*;
import sim.physics2D.physicalObject.*;
import sim.physics2D.util.*;

public class Can extends MobileObject2D implements Steppable
{
	public boolean visible;
	public Can(Double2D pos, Double2D vel)
	{
		// vary the mass with the size
		this.setPose(pos, new Angle(0));
		this.setVelocity(vel);
		this.setShape(new sim.physics2D.shape.Circle(2, Color.blue), 80);
		this.setCoefficientOfFriction(.5);
		this.setCoefficientOfRestitution(1);
		visible = true;
	}
 
	public void step(SimState state)
	{
		Double2D position = this.getPosition();
		PhysicsTutorial3 simPhysicsTutorial3 = (PhysicsTutorial3)state;
		simPhysicsTutorial3.fieldEnvironment.setObjectLocation(this, new sim.util.Double2D(position.x, position.y));
	}
}
</pre>
<p>
	The only thing interesting to note here is that the can has a coefficient of friction
	of .5. This is to simulate the fact that the can is heavy and is relatively hard to
	push.
</p>
<h2>Create the shell robot class</h2>
<p>
	In the <b>sim/app/physicsTutorial3</b> directory, create a file called <b>Bot.java</b>.
	In this file add:
</p>
<pre>

package sim.app.physicsTutorial3;

import java.awt.*;
import sim.physics2D.constraint.*;
import sim.engine.*;
import sim.physics2D.physicalObject.*;
import sim.physics2D.forceGenerator.ForceGenerator;
import sim.physics2D.util.*;
import sim.util.Bag;

public class Bot extends sim.robot2D.Robot implements Steppable, ForceGenerator
{
	Can currentCan;
	
	private ConstraintEngine objCE;
	private PinJoint pj;
	
	private Double2D canHome;
	private double canHomeRadius = 20;
	private Double2D botHome;
	private int botState;

	private final int HAVECAN = 1;
	private final int APPROACHINGCAN = 2;
	private final int RELEASINGCAN = 3;
	private final int RETURNINGHOME = 4;
	private final int SEARCHING = 5;
	
	public Effector e1;
	public Effector e2;
	
	public Bot(Double2D pos, Double2D vel)
	{
		// vary the mass with the size
		this.setPose(pos, new Angle(0));
		this.setVelocity(vel);
		this.setShape(new sim.physics2D.shape.Circle(10, Color.gray), 300);
		this.setCoefficientOfFriction(.2);
		this.setCoefficientOfRestitution(1);	
		
		this.currentCan = null;
		this.canHome = new Double2D(50, 50);
		this.botHome = pos;
		this.botState = SEARCHING;
		
		this.objCE = ConstraintEngine.getInstance();
	}
}

</pre>
<p>
	This code sets up the constructor for the <b>Bot</b> class and defines 
	some variables. Note the following:
	<ul>
		<li><b>Bot</b> inherits from <b>sim.app.robot2D.Robot</b>. <b>Robot</b>
			provides</li> several basic methods for coordinate frame transformation
			and control.
		<li>The <b>Bot</b> has two <b>Effector</b> objects defined. These 
			are the two small circles that serve as its gripper</li>
		<li>The <b>Bot</b>'s coefficient of friction is .2 - less than half of the <b>Can</b>'s
		value. This is to simulate the robot's wheeled locomotion.</li>
		<li>The robots decide what to do based on their state. The states can be 
		<b>HAVECAN</b>, <b>APPROACHINGCAN</b>, <b>RELEASINGCAN</b>, <b>RETURNINGHOME</b>, 
		and <b>SEARCHING</b></li>
	</ul>
</p>

<h2>Add the Bot's step method</h2>
<p>Add the following method to the <b>Bot</b> class:</p>
<pre>
	public void step(SimState state)
	{
		Double2D position = this.getPosition();
		PhysicsTutorial3 simPhysicsTutorial3 = (PhysicsTutorial3)state;
		simPhysicsTutorial3.fieldEnvironment.setObjectLocation(this, new sim.util.Double2D(position.x, position.y));
	
		// Find a can
		if (botState == SEARCHING)
		{
			Bag objs = simPhysicsTutorial3.fieldEnvironment.allObjects;
			objs.shuffle(state.random);
			for (int i = 0; i < objs.numObjs; i++)
			{
				if (objs.objs[i] instanceof Can)
				{
					currentCan = (Can)objs.objs[i];
					if (currentCan.getPosition().y > 50 && currentCan.visible)
					{ 
						botState = APPROACHINGCAN;	
						break;
					}
					else
						currentCan = null; // can is already home or has been picked
											// up by another bot
				}
			}
			
			if (currentCan == null)
				botState = RETURNINGHOME;
		}
	}
</pre>
<p>
	At each time step, the robot tries to find a can to collect if its current state is
	<b>SEARCHING</b>. If it can't find an eligible can (which means they have all been
	collected), it returns to its starting position.
</p>

<h2>Add the Bot's handleCollisions method</h2>
<p>Add the following method to the <b>Bot</b> class:</p>
<pre>
	public int handleCollision(PhysicalObject2D other, Double2D colPoint)
	{
		Double2D globalPointPos = this.getPosition().add(colPoint);
		Double2D localPointPos = this.localFromGlobal(globalPointPos);
		Angle colAngle = this.getAngle(localPointPos);
		
		// Make sure the object is a can and that it is (roughly) between
		// the effectors
		if (other instanceof Can && botState == APPROACHINGCAN
				&& (colAngle.radians < Math.PI / 8 || colAngle.radians > (Math.PI * 2 - Math.PI / 8)))
		{
			// Create a fixed joint directly at the center of the can
			pj = new PinJoint(other.getPosition(), this, other);
			objCE.registerForceConstraint(pj);
			
			botState = HAVECAN;
			
			objCE.setNoCollisions(this, other);
			objCE.setNoCollisions(e1, other);
			objCE.setNoCollisions(e2, other);
			
			currentCan.visible = false;
			return 2; // sticky collision
		}
		else
			return 1; // regular collision
	}
</pre>
<p>
	The physics engine notifies an object if it is involved in a collision by 
	calling its <b>handleCollision</b> method. This method is defined by the base
	class <b>sim.physics2D.physicalObject.PhysicalObject2D</b>. An object can
	choose to override this method to perform actions when a collision occurs. After 
	being notified of a collision, the object can choose to turn off collision response
	for the collision by returning 0, respond to the collision as normal by returning 1 (this is the default behavior), 
	or set the collision as "sticky" by returning 2. A "sticky" collision is a perfectly
	inelastic one (like a flat basketball) in which all energy is lost along the
	collision normal.
</p>
<p>
	The physics engine passes the object the collision point for the current collision. 
	The passed in variable represents a vector from the object's 
	position (center of mass) to the collision point. The vector is defined in the 
	global coordinate frame, so the first thing the object does in this method is 
	get the collision point in its local coordinate frame using the <b>localFromGlobal</b>
	method provided by <b>Robot</b>.
</p>
<p>
	After getting the local coordinate of the collision point, the bot checks to see
	if this collision represents an attempt to pick up a can. For this to happen, the
	other object must be a <b>Can</b> and the collision must occur between the two circles
	representing the robot's effector.
</p>
<p>
	If the collision is an attempt to pick up a can, the bot "picks up" the can
	by dynamically constraining the can to itself with a pin joint. It then turns
	off collisions between the can and itself and its effector using 
	<b>sim.physics2D.constraint.ConstraintEngine</b>'s <b>setNoCollisions</b> method. This 
	tells the collision detector to no longer consider these pairs
	of objects. This method can be used for any pair of objects for which collision detection
	and response is not desired. Finally, the method sets this collision as "sticky" to
	prevent the collision response from attempting to bounce the can off the bot
	(since we are trying to simulate the bot grabbing the can).
</p>
<p>If this collision is not an attempt to pick up a can, the method returns 1 for normal
collision response.</p>

<h2>Add the Bot's addForce method</h2>
<p>Add the following method to the <b>Bot</b> class:</p>
<pre>
	public void addForce()
	{
		switch (botState)
		{
			case HAVECAN:
				if (this.getPosition().y <= 40)
				{
					if (this.getVelocity().length() > 0.01 || this.getAngularVelocity() > 0.01)
						this.stop();
					else
					{
						objCE.unRegisterForceConstraint(pj);				
						botState = RELEASINGCAN;
						objCE.removeNoCollisions(this, currentCan);
						objCE.removeNoCollisions(e1, currentCan);
						objCE.removeNoCollisions(e2, currentCan);
						currentCan.visible = true;
					}
				}
				else
					this.goTo(new Double2D(this.getPosition().x, 40));
				break;
			case RELEASINGCAN:
				// back out of can home
				if (this.getPosition().subtract(currentCan.getPosition()).length() <= 30)
					backup();
				else
					botState = SEARCHING;
				break;
			case APPROACHINGCAN:
				if (currentCan.visible)
					this.goTo(currentCan.getPosition());
				else
					botState = SEARCHING;
				break;
			case RETURNINGHOME:
				if (this.getPosition().subtract(botHome).length() <= 30)
				{
					if (this.getOrientation().radians != 0)
						this.faceTowards(new Angle(0));
					else
						stop();
				}
				else
					this.goTo(botHome);
				break;	
		}

	}
</pre>
<p>Recall from physics tutorial 2 that any forces applied to objects should be 
	applied from within a <b>ForceGenerator</b>'s <b>addForce</b> method. This
	method is called at least once per time step by the numerical integrator.</p>
<p>
	This method uses the controller functionality provided by sim.app.robot2D.<b>Robot</b> to
	control the robot based on its current state. If the bot currently has a can
	and is at the top of the screen, it first stops and then releases the can by 
	dynamically unregistering the pin joint. If it has just released the can, it backs up
	and finds a new can to collect. If the bot has found a can (state <b>APPROACHINGCAN</b>), 
	it moves toward it. Finally, if there are no more cans to collect (state <b>RETURNINGHOME</b>), 
	the bot returns to its starting position and stops.
</p>
<h2>Set up the physics simulation in the MASON <b>SimState</b>'s start method</h2>
<p>
	The file <b>sim/app/physicsTutorial3/PhysicsTutorial3.java</b> already has much of 
	the non-physics related MASON code needed to set up the simulation. The only code
	to add is in the <b>start</b> method. Add the following code to the <b>start</b>
	method after the "Add physics specific code here" line.
</p>
<pre>
		// Create and schedule the physics engine
		PhysicsEngine2D objPE = new PhysicsEngine2D();
		schedule.scheduleRepeating(objPE);
		
		Double2D pos;
		Double2D vel;
		
		Bot bot;
		Can can;
		Wall wall;

		// WALLS
		// HORIZ
		pos = new Double2D(100,0);
		wall = new Wall(pos, 193, 6);
		fieldEnvironment.setObjectLocation(wall, new sim.util.Double2D(pos.x, pos.y));
		objPE.register(wall);
 
		pos = new Double2D(100,200);
		wall = new Wall(pos, 193, 6);
		fieldEnvironment.setObjectLocation(wall, new sim.util.Double2D(pos.x, pos.y));
		objPE.register(wall);
		
		// VERT
		pos = new Double2D(0,100);
		wall = new Wall(pos, 6, 200);
		fieldEnvironment.setObjectLocation(wall, new sim.util.Double2D(pos.x, pos.y));
		objPE.register(wall);
	
		pos = new Double2D(200,100);
		wall = new Wall(pos, 6, 200);
		fieldEnvironment.setObjectLocation(wall, new sim.util.Double2D(pos.x, pos.y));
		objPE.register(wall);
		
		// Set up the cans
		for (int i = 0; i < 4; i++)
		{
			double x = Math.max(Math.min(random.nextDouble() * 200, 190), 10);
			double y = Math.max(Math.min(random.nextDouble() * 200, 190), 60);
			
			pos = new Double2D(x, y);
			
			can = new Can(pos, new Double2D(0, 0));
			fieldEnvironment.setObjectLocation(can, new sim.util.Double2D(pos.x, pos.y));
			objPE.register(can);
			schedule.scheduleRepeating(can);
		}
		
		// Set up the bots
		for (int i = 0; i < 2; i++)
		{
			double x = Math.max(Math.min(random.nextDouble() * 200, 180), 20);
			double y = Math.max(Math.min(random.nextDouble() * 200, 180), 50);
			
			pos = new Double2D(x, y);
			bot = new Bot(pos, new Double2D(0, 0));
			objPE.register(bot);
			fieldEnvironment.setObjectLocation(bot, new sim.util.Double2D(pos.x, pos.y));
			schedule.scheduleRepeating(bot);
			
			Effector effector;
			
			pos = new Double2D(x + 12, y + 6);
			effector = new Effector(pos, new Double2D(0, 0), 1, Color.gray);
			objPE.register(effector);
			fieldEnvironment.setObjectLocation(effector, new sim.util.Double2D(pos.x, pos.y));
			schedule.scheduleRepeating(effector);
			bot.e1 = effector;
			
			objPE.setNoCollisions(bot, effector);

			PinJoint pj = new PinJoint(pos, effector, bot);	
			objPE.register(pj);
			
			pos = new Double2D(x + 12, y - 6);
			effector = new Effector(pos, new Double2D(0, 0), 1, Color.gray);
			objPE.register(effector);
			fieldEnvironment.setObjectLocation(effector, new sim.util.Double2D(pos.x, pos.y));
			schedule.scheduleRepeating(effector);
			bot.e2 = effector;
			
			objPE.setNoCollisions(bot, effector);
	
			pj = new PinJoint(pos, effector, bot);
			objPE.register(pj);		
		}
</pre>
<p>
	This code is similar to the <b>start</b> methods in tutorials 1 and 2. It creates
	and schedules the physics engine, creates and places the walls, and creates the cans
	and bots.
</p>
<h2>Run the simulation</h2>
<p>
	Compile Can.java, Bot.java, Wall.java, Effector.java, PhysicsTutorial3.java, 
	and PhysicsTutorial3WithUI.java. Then run the program 
	as <b>java sim.app.physicsTutorial3.PhysicsTutorial3WithUI</b>.
</p>
<p>
	After pressing play, you should see two gray robots attempting to 
	move four blue cans to the top of the screen.
</p>

